#include "cx_message_queue.h"
#include "cx_heap.h"
#include "cx_typedef.h"
#define LOG_TAG "[MSG_QE]"
#include "log.h"
/* Test code
#include <stdio.h>
#define OUTPUT_QUEUE_STATUS(x) \
printf("free space:%d,front:%d,rear:%d\n",message_queue_status(&x),x.front,x.rear)
*/

#define cx_msg_queue_sem NULL

int message_queue_status(MessageQueue_t *queue)
{
    int ret;
    int sub = queue->rear - queue->front;
    if(sub < 0)
    {
        ret = 0-sub-1;
    }
    else
    {
        ret = queue->capacity - sub - 1;
    }

    if(queue->busy)
    {
        ret = 0-ret;
    }
    return ret;
}

static char message_queue_dequeue_front_go_forward(MessageQueue_t *queue)
{
    char value = queue->buf[queue->front], max_num = queue->capacity - 1;
    CX_OS_SEM_WAIT(queue->sem);
    if(queue->front >= max_num)
        queue->front = 0;
    else
        ++(queue->front);
    CX_OS_SEM_SIGNAL(queue->sem);
    return value;
}

static void message_queue_enqueue_rear_go_forward(MessageQueue_t *queue, char value)
{
    char max_num = queue->capacity - 1;
    if(!queue)
    {
        return;
    }
    CX_OS_SEM_WAIT(queue->sem);
    queue->buf[queue->rear] = value;
    if(queue->rear >= max_num)
        queue->rear = 0;
    else
        ++(queue->rear);
    CX_OS_SEM_SIGNAL(queue->sem);
    return;
}

MessageQueue_t* message_queue_new(cx_size_t queue_size)
{
    MessageQueue_t *queue;
    queue = (MessageQueue_t*)cx_malloc(sizeof(MessageQueue_t));
    if(queue == NULL)
    {
        return NULL;
    }
    queue->buf = (char*)cx_malloc(queue_size);
    if(queue->buf == NULL)
    {
        cx_free(queue);
        return NULL;
    }
    queue->capacity = queue_size;
    queue->front = 0;
    queue->rear = 0;
    queue->busy = 0;
    return queue;
}

void message_queue_free(MessageQueue_t *queue)
{
    CX_OS_SEM_WAIT(queue->sem);
    cx_free(queue->buf);
    cx_free(queue);
}

int message_queue_dequeue(MessageQueue_t *queue, void *buf, cx_uint32_t buf_size)
{
    int denums;
    uint16_t queue_msg_size;
    if(message_queue_status(queue) == queue->capacity)
    {
        denums = 0;
        LOGD("denums1:%d", 0);
    }
    else
    {
        ((cx_uint8_t*)(&queue_msg_size))[0] = message_queue_dequeue_front_go_forward(queue);
        ((cx_uint8_t*)(&queue_msg_size))[1] = message_queue_dequeue_front_go_forward(queue);
        denums = queue_msg_size;
        denums = (denums > buf_size)?buf_size:denums;
        LOGD("denums2:%d", denums);
        CX_OS_SEM_WAIT(queue->sem);
        for(int i=0; i<denums;i++)
        {
            ((char*)buf)[i] = message_queue_dequeue_front_go_forward(queue);
            LOGD("deque:%d", ((char*)buf)[i]);
        }
        CX_OS_SEM_SIGNAL(queue->sem);
    }

    return denums;
}

int message_queue_enqueue(MessageQueue_t *queue, void *buf, cx_uint16_t size)
{
    int free = message_queue_status(queue);
    int ret = 0;
    if(free > size)
    {
        CX_OS_SEM_WAIT(queue->sem);
        message_queue_enqueue_rear_go_forward(queue, ((cx_uint8_t*)(&size))[0]);
        message_queue_enqueue_rear_go_forward(queue, ((cx_uint8_t*)(&size))[1]);
        for(int i=0; i<size; i++)
        {
            LOGD("Enque:%d", ((char*)buf)[i]);
            message_queue_enqueue_rear_go_forward(queue, ((char*)buf)[i]);
        }
        CX_OS_SEM_SIGNAL(queue->sem);
        ret = size + 1;
    }

    return ret;
}

/* Test
int main(void)
{
    char *a = "abc";
    char *b = "def";
    char *c = "ghijklmn";
    char buf[40];
    MessageQueue_t queue;
    message_queue_init(&queue);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_enqueue(&queue,a,4);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_enqueue(&queue,b,4);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_dequeue(&queue,buf);
    printf("output:%s\n",buf);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_dequeue(&queue,buf);
    printf("output:%s\n",buf);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_enqueue(&queue,a,4);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_enqueue(&queue,b,4);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_enqueue(&queue,c,9);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_enqueue(&queue,c,9);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_dequeue(&queue,buf);
    printf("output:%s\n",buf);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_dequeue(&queue,buf);
    printf("output:%s\n",buf);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_dequeue(&queue,buf);
    printf("output:%s\n",buf);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_dequeue(&queue,buf);
    printf("output:%s\n",buf);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_dequeue(&queue,buf);
    printf("output:%s\n",buf);
    OUTPUT_QUEUE_STATUS(queue);

    message_queue_dequeue(&queue,buf);
    printf("output:%s\n",buf);
    OUTPUT_QUEUE_STATUS(queue);
    return 0;
}

*/
